#!/usr/bin/env python3
import rospy
import sys
import rosbag
from message_filters import ApproximateTimeSynchronizer, Subscriber
from sensor_msgs.msg import PointCloud2
from sensor_msgs import point_cloud2
import open3d as o3d
from nav_msgs.msg import Odometry
import os
from parse import parse
import numpy as np
import quaternion



sys.path.append("/workspace/ws_marsim_modify/devel/lib")

import libpy_pointcloud

inputfolder = "/workspace/bags/crane/"
outputfile = "/workspace/bags/crane.pcd"


def readPosFromFile(filepath):
    with open(filepath, 'r') as file:
        line = file.readline()
        #parse xyz
        x,y,z = parse("Position: {}, {}, {}",line)
        r=np.array([float(x),float(y),float(z)])
        line = file.readline()
        #parse xyzw
        qx,qy,qz,qw = parse("Orientation: {}, {}, {}, {}",line)
        # print(f'{x,y,z}')
        q = np.array([float(qw),float(qx),float(qy),float(qz)])
        return [r,q]

def main():
    
    pcd_trans = o3d.geometry.PointCloud()
    all_points = None
    files_and_folders = os.listdir(inputfolder)
    # files = [file for file in files_and_folders if os.path.isfile(os.path.join(inputfolder, file))]
    count_file = 0
    for filename in files_and_folders:
        file_extension = filename.split('.')[-1]
        file_stem = filename.split('.')[0]
        
        if file_extension == 'pcd':
            #read pts
            pcd = o3d.io.read_point_cloud(inputfolder+file_stem+".pcd")
            
            #read pos
            [r,q] = readPosFromFile(inputfolder+file_stem+".pos")
            trans_pts = libpy_pointcloud.transformpoints(pcd.points,q,r)
            if all_points is None:
                all_points = trans_pts
                # print(all_points.shape)
            else:
                all_points = np.concatenate((all_points, trans_pts), axis=0)
                print(all_points.shape)
                
            count_file = count_file+1
        
        
        if count_file > 100:
            break
        

    pcd_trans.points = o3d.utility.Vector3dVector(all_points)  
    o3d.io.write_point_cloud(outputfile, pcd_trans)          


if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass